#include <Servo.h>
#include <PS2X_lib.h>	//for v1.6
#include <Timer.h>

/******************************************************************
 * set pins connected to PS2 controller:
 *	 - 1e column: original 
 *	 - 2e colmun: Stef?
 * replace pin numbers by the ones you use
 ******************************************************************/
#define PS2_DAT				A4	//14		
#define PS2_CMD				A0	//15
#define PS2_SEL				A1	//16
#define PS2_CLK				A2	//17

/******************************************************************
 * select modes of PS2 controller:
 *	 - pressures = analog reading of push-butttons 
 *	 - rumble		= motor rumbling
 * uncomment 1 of the lines for each mode selection
 ******************************************************************/
//#define pressures	 true
#define pressures	 false
//#define rumble			true
#define rumble			false

Servo myservo;
PS2X ps2x; // create PS2 Controller Class
Timer timer;

//right now, the library does NOT support hot pluggable controllers, meaning 
//you must always either restart your Arduino after you connect the controller, 
//or call config_gamepad(pins) again after connecting the controller.

int error = 0;
byte type = 0;
byte vibrate = 0;

byte rotation = 90;
byte rotationTarget = 90;

#define SERVO_PIN 3
#define PWM            5   //Motor A's pin
#define DIR            4   //Motor A's pin

void servoRotation()
{
	// left
	if (rotationTarget < rotation)
	{
		byte step = rotation > 90 ? 4 : 7;

		if (rotation - rotationTarget >= step)
		{
			rotation -= step;
			myservo.write(rotation);
		}
	}
	else if (rotationTarget > rotation)
	{
		byte step = rotation < 90 ? 7 : 4;
		if (rotationTarget - rotation >= step)
		{
			rotation += step;
			myservo.write(rotation);
		}
	}
}

void setup()
{
	Serial.begin(9600);

	pinMode(PWM, OUTPUT); // Pin state
	pinMode(DIR, OUTPUT);

	myservo.attach(SERVO_PIN); // attaches the servo on pin 9 to the servo object

	myservo.write(90 + 30);
	delay(500);

	myservo.write(90 - 30);
	delay(500);

	myservo.write(90);

	timer.every(20, servoRotation);

	delay(2000); //added delay to give wireless ps2 module some time to startup, before configuring it

	//CHANGES for v1.6 HERE!!! **************PAY ATTENTION*************

	//setup pins and settings: GamePad(clock, command, attention, data, Pressures?, Rumble?) check for error
	error = ps2x.config_gamepad(PS2_CLK, PS2_CMD, PS2_SEL, PS2_DAT, pressures, rumble);

	if (error != 0) {
		for (int i = 0; i < 5; i++) {
			error = ps2x.config_gamepad(PS2_CLK, PS2_CMD, PS2_SEL, PS2_DAT, pressures, rumble);
			if (error == 0) break;

			delay(200);
		}
	}

	type = ps2x.readType();
}

void loop()
{
	timer.update();

	/* You must Read Gamepad to get new values and set vibration values
		 ps2x.read_gamepad(small motor on/off, larger motor strenght from 0-255)
		 if you don't enable the rumble, use ps2x.read_gamepad(); with no values
		 You should call this at least once a second
	 */
	if (error == 1) //skip loop if no controller found
		return;

	if (true)
	{									   //DualShock Controller
		ps2x.read_gamepad(false, vibrate); //read controller and set large motor to spin at 'vibrate' speed

		if (true)
		{ //print stick values if either is TRUE
			byte lx = ps2x.Analog(PSS_LX);
			byte ly = ps2x.Analog(PSS_RY);

			// Serial.println(ps2x.Analog(PSS_LY), DEC);

#if 0
			Serial.print("Stick Values:");
			Serial.print(ps2x.Analog(PSS_LY), DEC); //Left stick, Y axis. Other options: LX, RY, RX	
			Serial.print(",");
			Serial.print(lx, DEC); 
			Serial.print(",");
			Serial.print(ps2x.Analog(PSS_RY), DEC); 
			Serial.print(",");
			Serial.println(ps2x.Analog(PSS_RX), DEC);
#endif

			byte degree = 90;

			if (lx >= 0 && lx <= 128)
			{
				degree = map(lx, 0, 128, 90 - 45 - 25, 90); // 70
			}
			else if (lx >= 129 && lx <= 255)
			{
				degree = map(lx, 128, 255, 90, 90 + 45 - 5); // 40
			}

			if (ly >= 0 && ly <= 100)
			{
				digitalWrite(DIR, HIGH);
				analogWrite(PWM, map(ly, 0, 100, 255, 60));
			}
			else if (ly >= 156 && ly <= 255)
			{
				digitalWrite(DIR, LOW);
				analogWrite(PWM, map(ly, 156, 255, 60, 255));
			}
			else
			{
				analogWrite(PWM, 0);
			}

			// myservo.write(degree);
			rotationTarget = degree;
			// Serial.println(degree, DEC);
		}
	}
	delay(50);
}
